from Source.CWJ import qyhf
from Methods.tools import *
import copy
from Method import blocks_select
from Avoidance_CURD import Pi_bypass, pilist_avoidance, Set_slope, LinkedList
# from Source.Astar20221013.main import astar
from avoiding_obstacles import *

# 原来管道组排管算法,不含三通管道排管
def pipegroup_main(pipeBase, blockBase, block_arr, PipeGroup_info, new_allpipelist, flag,output_pip_Link):
    PipeGroupNo = PipeGroup_info[0]
    PipeGroup_Type = PipeGroup_info[1]
    ceilling = PipeGroup_info[2]
    floor = PipeGroup_info[3]
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for pipeList in PipeGroup._pipeLists:
                pilist = PipeListNo_to_PipeList(PipeGroupNo, pipeList.pipeListNo, pipeBase)
                pip_info = PipeListNo_to_info(PipeGroupNo, pipeList.pipeListNo, pipeBase)
                print('旧管道是', pilist)
                blocks_selection = blocks_select(pilist, block_arr, 5000)
                if flag == 0:
                    block_wall_list_recursion = copy.deepcopy(blocks_selection)
                    slope_change_flag, pilist = Set_slope(pilist, PipeGroup_Type, ceilling) #为原始管道设置斜率
                    linkedlist = LinkedList() #创建双向链表
                    linkedlist.create_LinkedList(pilist) #初始化双向链表
                    change_flag1 = pilist_avoidance(linkedlist, blocks_selection, blockBase, PipeGroup_Type, ceilling, pipeBase) #用于同一列中的管道避障，change_flag1为避障标志
                    
                    new_pipeList = linkedlist.output_Arr()
                    change_flag2, new_pipeList, position_list,if_avoid = Pi_bypass(new_pipeList, blocks_selection, blockBase, block_wall_list_recursion, PipeGroup_Type, ceilling)
                    print('change_flag1',change_flag1)
                    change_flag = change_flag1 or change_flag2
                    print('change_flag',change_flag)

                    one_group_pipelines = output(if_avoid, new_pipeList, pip_info)

                    double_update(new_pipeList, pipeBase,PipeGroupNo, pipeList.pipeListNo, position_list, change_flag, slope_change_flag) #将二通管道数组信息更新至管道类及增加连接关系
                    print('新管道是:', new_pipeList)

                else:
                    new_pipeList = astar(pilist, blocks_selection, 20, 20, floor, ceilling[0], ceilling[1])
                    double_update(new_pipeList, pipeBase,PipeGroupNo, pipeList.pipeListNo, position_list = [[], []], change_flag = True, slope_change_flag = True) #将二通管道数组信息更新至管道类及增加连接关系
                    print('新管道是', new_pipeList)
                output_pip_Link.append(one_group_pipelines)
                # 每生成一列管道，就存进数组中，最后把所有的新管道都存进去
                new_allpipelist.append(new_pipeList)
                new_blocklist = qyhf.pipes_to_blocks(new_pipeList, 15)
                # # 管道类型的障碍物，添加到障碍物对象中
                add(new_blocklist, blockBase)
                # # 管道类型的障碍物，添加到障碍物数组中
                block_arr.extend(new_blocklist)
                    
#水管三通排管
def pipelist_main(flag, pilist, block_arr, blockBase, PipeGroup_Type, ceilling, pipeBase):
    blocks_selection = blocks_select(pilist, block_arr, 5000)
    block_list_leave = copy.deepcopy(blocks_selection)
    print('旧管道是', pilist)
    slope_change_flag, pilist = Set_slope(pilist, PipeGroup_Type, ceilling) #为原始管道设置斜率
    linkedlist = LinkedList() #创建双向链表
    linkedlist.create_LinkedList(pilist) #初始化双向链表
    change_flag1 = pilist_avoidance(linkedlist, blocks_selection, blockBase, PipeGroup_Type, ceilling, pipeBase) #用于同一列中的管道避障
    new_pipeList = linkedlist.output_Arr()
    change_flag2, new_pipeList, position_list,if_avoid  = Pi_bypass(new_pipeList, blocks_selection, blockBase, block_list_leave, PipeGroup_Type, ceilling)
    change_flag = change_flag1 or change_flag2
    print('新管道是:', new_pipeList)
    return change_flag, slope_change_flag, new_pipeList, position_list

#风管三通排管
def duct_pipelist_main(flag_arr, pilist, block_arr, blockBase, PipeGroup_Type, ceilling, pipeBase):
    blocks_selection = blocks_select(pilist, block_arr, 5000)
    block_list_leave = copy.deepcopy(blocks_selection)
    print('旧管道是:', pilist)
    slope_change_flag, pilist = Set_slope(pilist, PipeGroup_Type, ceilling) #为原始管道设置斜率
    linkedlist = LinkedList() #创建双向链表
    linkedlist.create_LinkedList(pilist) #初始化双向链表
    change_flag1 = pilist_avoidance(linkedlist, blocks_selection, blockBase, PipeGroup_Type, ceilling, pipeBase) #用于同一列中的管道避障
    new_pipeList = linkedlist.output_Arr()
    change_flag2, new_pipeList, position_list,if_avoid  = Pi_bypass(new_pipeList, blocks_selection, blockBase, block_list_leave, PipeGroup_Type, ceilling)
    change_flag = change_flag1 or change_flag2
    print('新管道是:', new_pipeList)
    return change_flag, slope_change_flag, new_pipeList, position_list

def double_update(new_pipeList, pipeBase, PipeGroupNo, PipeListNo, position_list, change_flag, slope_change_flag): #将二通管道数组信息更新至管道类、增加连接关系、添加标球信息
    from Source.HJL.PipeClass import Pipe
    from Method import get_Unit_vector, rotation_2D
    import math
    
    #添加标球信息
    cross_position_list = position_list[0]
    fail_position_list = position_list[1]
    for cross_postion in cross_position_list:
        pipeBase.yellowBallCoordinates.append(cross_postion)
    for fail_position in fail_position_list:
        pipeBase.blueBallCoordinates.append(fail_position)

    if change_flag or slope_change_flag:
        for PipeGroup in pipeBase._pipeGroups:
            if PipeGroup.pipeGroupNo == PipeGroupNo:                   
                for PipeList in PipeGroup._pipeLists:
                    if PipeList.pipeListNo == PipeListNo:
                        PipeList._pipes.clear()
                        for i in range(len(new_pipeList)):
                            pipe = Pipe()
                            pipe.uid = str(pipeBase.uniqueId)
                            pipeBase.uniqueId += 1
                            pipe.start = new_pipeList[i][0]
                            pipe.end = new_pipeList[i][1]
                            pipe.size.width = new_pipeList[i][2]
                            pipe.size.height = new_pipeList[i][3]
                            if new_pipeList[i][4] != None:
                                angle = new_pipeList[i][4]
                                tan_value1 = math.tan(angle)
                                tranform_2 = [1, tan_value1, 0]
                                tranform_2 = get_Unit_vector(tranform_2)
                                if new_pipeList[i][0][2] > new_pipeList[i][1][2]:
                                    tranform_3 = rotation_2D(tranform_2, -90)
                                else:
                                    tranform_3 = rotation_2D(tranform_2, 90)
                                if new_pipeList[i][0][2] > new_pipeList[i][1][2]: #若起点z坐标大于终点z坐标，第三个法向量指向z-
                                    tranform_1 = [0, 0, -1]
                                else:
                                    tranform_1 = [0, 0, 1]
                                temp1 = ','.join([str(tempFloat) for tempFloat in tranform_1])
                                temp2 = ','.join([str(tempFloat) for tempFloat in tranform_2])
                                temp3 = ','.join([str(tempFloat) for tempFloat in tranform_3])
                                location = ','.join([str(coordinate / 304.8) for coordinate in new_pipeList[i][0]] )
                                pipe.transform = ';'.join([location, temp1, temp2, temp3])
                            PipeList._pipes.append(pipe)
    else: #若未发生避障或未设置斜率
        for PipeGroup in pipeBase._pipeGroups:
            if PipeGroup.pipeGroupNo == PipeGroupNo:                   
                for PipeList in PipeGroup._pipeLists:
                    if PipeList.pipeListNo == PipeListNo:
                        for i in range(len(new_pipeList)):
                            PipeList._pipes[i].start = new_pipeList[i][0]
                            PipeList._pipes[i].end = new_pipeList[i][1]

    #增加连接关系
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:                                
            for pipeList in PipeGroup._pipeLists:
                if pipeList.pipeListNo == PipeListNo:
                    if len(pipeList._nodes) == 0: #该列为实体管道
                        if change_flag or slope_change_flag: #若该列管道发生改变，则全部使用默认类型的连接件    
                            if len(pipeList._pipes) > 1:
                                for i in range(len(pipeList._pipes) - 1):
                                    PipeGroup.connectors.append([[pipeList._pipes[i].uid, pipeList._pipes[i + 1].uid], 'Default', 'Default'])
                    else: #该列为点线
                        if change_flag: #若发生避障，则使用默认类型的连接件
                            if len(pipeList._pipes) > 1:
                                for i in range(len(pipeList._pipes) - 1):
                                    PipeGroup.connectors.append([[pipeList._pipes[i].uid, pipeList._pipes[i + 1].uid], 'Default', 'Default'])
                        else: #若未发生避障，无论是否设置了斜率，则根据输入信息增加连接关系
                            for i in range(len(pipeList._pipes) - 1):
                                PipeGroup.connectors.append([[pipeList._pipes[i].uid, pipeList._pipes[i + 1].uid], pipeList._nodes[i].familyName, pipeList._nodes[i].symbolName])
                            
#由管道列标号,返回该列管道数组
def PipeListNo_to_PipeList(PipeGroupNo,PipeListNo,pipeBase):
    a = []
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for pipeList in PipeGroup._pipeLists:
                if pipeList.pipeListNo == PipeListNo:
                    for pipe in pipeList._pipes:
                        a.append([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle])
    return a